
// import oimo.collision.raycast.*;
// import oimo.common.MathUtil;
// import oimo.common.Transform;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { ConvexGeometry } from "./ConvexGeometry";
import { GeometryType } from "./GeometryType";
import { MathUtil } from "../../common/MathUtil";
import { M } from "../../common/M";
import { Aabb } from "./Aabb";
import { Transform } from "../../common/Transform";
import { Vec3 } from "../../common/Vec3";
import { RayCastHit } from "./RayCastHit";

/**
 * A cone collision geometry aligned with the y-axis.
 */

export class ConeGeometry extends ConvexGeometry {
	_radius: number;
	_halfHeight: number;

	sinTheta: number;
	cosTheta: number;

	/**
	 * Creates a cone collision geometry of radius `radius` and half-height `halfHeight`.
	 */
	constructor(radius: number, halfHeight: number) {
		super(GeometryType._CONE);
		this._radius = radius;
		this._halfHeight = halfHeight;
		this.sinTheta = radius / MathUtil.sqrt(radius * radius + 4 * halfHeight * halfHeight);
		this.cosTheta = 2 * halfHeight / MathUtil.sqrt(radius * radius + 4 * halfHeight * halfHeight);
		this._updateMass();
	}

	/**
	 * Returns the radius of the cone.
	 */
	public getRadius(): number {
		return this._radius;
	}

	/**
	 * Returns the half-height of the cone.
	 */
	public getHalfHeight(): number {
		return this._halfHeight;
	}

	public _updateMass(): void {
		var r2: number = this._radius * this._radius;
		var h2: number = this._halfHeight * this._halfHeight * 4;
		this._volume = MathUtil.PI * r2 * this._halfHeight * 2 / 3;
		M.mat3_diagonal(this._inertiaCoeff,
			1 / 20 * (3 * r2 + 2 * h2),
			3 / 10 * r2,
			1 / 12 * (3 * r2 + 2 * h2)
		);
	}

	public _computeAabb(aabb: Aabb, tf: Transform): void {
		var axis: Vec3 = new Vec3();
		var axis2: Vec3 = new Vec3();
		var eh: Vec3 = new Vec3();
		var er: Vec3 = new Vec3();
		M.mat3_getCol(axis, tf._rotation, 1);
		M.vec3_compWiseMul(axis2, axis, axis);

		var axis2x: number = M.vec3_get(axis2, 0);
		var axis2y: number = M.vec3_get(axis2, 1);
		var axis2z: number = M.vec3_get(axis2, 2);

		M.vec3_set(er, MathUtil.sqrt(1 - axis2x), MathUtil.sqrt(1 - axis2y), MathUtil.sqrt(1 - axis2z));
		M.vec3_scale(er, er, this._radius);

		M.vec3_scale(eh, axis, this._halfHeight);

		var rmin: Vec3 = new Vec3(); // -(signed projected axis) - (projected radius)
		var rmax: Vec3= new Vec3(); // -(signed projected axis) + (projected radius)
		M.vec3_negate(rmin, eh);
		M.vec3_sub(rmin, rmin, er);
		M.vec3_negate(rmax, eh);
		M.vec3_add(rmax, rmax, er);

		var max: Vec3 = new Vec3();
		var min: Vec3 = new Vec3();
		M.vec3_max(max, rmin, rmax);
		M.vec3_max(max, max, eh);
		M.vec3_min(min, rmin, rmax);
		M.vec3_min(min, min, eh);

		M.vec3_add(aabb._min, tf._position, min);
		M.vec3_add(aabb._max, tf._position, max);
	}

	public computeLocalSupportingVertex(dir: Vec3, out: Vec3): void {
		var dx: number = dir.x;
		var dy: number = dir.y;
		var dz: number = dir.z;
		if (dy > 0 && dy * dy > this.sinTheta * this.sinTheta * (dx * dx + dy * dy + dz * dz)) {
			out.init(0, this._halfHeight - this._gjkMargin / this.sinTheta, 0);
			if (out.y < 0) out.y = 0;
			return;
		}
		var rx: number = dir.x;
		var rz: number = dir.z;
		var len: number = rx * rx + rz * rz;
		var height: number = 2 * this._halfHeight;
		var coreRadius: number = (height - this._gjkMargin) / height * this._radius - this._gjkMargin / this.cosTheta;
		if (coreRadius < 0) coreRadius = 0;
		var invLen = len > 0 ? coreRadius / MathUtil.sqrt(len) : 0;
		var coreHalfHeight: number = this._halfHeight - this._gjkMargin;
		if (coreHalfHeight < 0) coreHalfHeight = 0;
		out.x = rx * invLen;
		out.y = -coreHalfHeight;
		out.z = rz * invLen;
	}

	public _rayCastLocal(begin: Vec3, end: Vec3, hit: RayCastHit): boolean {
		var p1x: number = M.vec3_get(begin, 0);
		var p1y: number = M.vec3_get(begin, 1);
		var p1z: number = M.vec3_get(begin, 2);
		var p2x: number = M.vec3_get(end, 0);
		var p2y: number = M.vec3_get(end, 1);
		var p2z: number = M.vec3_get(end, 2);
		var halfH: number = this._halfHeight;
		var dx: number = p2x - p1x;
		var dy: number = p2y - p1y;
		var dz: number = p2z - p1z;

		// Y
		var tminy: number = 0;
		var tmaxy: number = 1;
		if (dy > -1e-6 && dy < 1e-6) {
			if (p1y <= -halfH || p1y >= halfH) {
				return false;
			}
		} else {
			var invDy: number = 1 / dy;
			var t1: number = (-halfH - p1y) * invDy;
			var t2: number = (halfH - p1y) * invDy;
			if (t1 > t2) {
				var tmp: number = t1;
				t1 = t2;
				t2 = tmp;
			}
			if (t1 > 0) tminy = t1;
			if (t2 < 1) tmaxy = t2;
		}
		if (tminy >= 1 || tmaxy <= 0) return false;

		// XZ
		var tminxz: number = 0;
		var tmaxxz: number = 0;

		p1y -= halfH; // translate so that the new origin be (0, -halfH, 0)

		var cos2: number = this.cosTheta * this.cosTheta;
		var a: number = cos2 * (dx * dx + dy * dy + dz * dz) - dy * dy;
		var b: number = cos2 * (p1x * dx + p1y * dy + p1z * dz) - p1y * dy;
		var c: number = cos2 * (p1x * p1x + p1y * p1y + p1z * p1z) - p1y * p1y;
		var D: number = b * b - a * c;
		if (a != 0) {
			if (D < 0) return false;
			var sqrtD: number = MathUtil.sqrt(D);
			if (a < 0) {
				// ((-inf, t1) union (t2, +inf)) join (0, 1)
				if (dy > 0) {
					// (0, t1)
					tminxz = 0;
					tmaxxz = (-b + sqrtD) / a;
					if (tmaxxz <= 0) return false;
				} else {
					// (t2, 1)
					tminxz = (-b - sqrtD) / a;
					tmaxxz = 1;
					if (tminxz >= 1) return false;
				}
			} else {
				// (t1, t2) join (0, 1)
				tminxz = (-b - sqrtD) / a;
				tmaxxz = (-b + sqrtD) / a;
				if (tminxz >= 1 || tmaxxz <= 0) return false;
			}
		} else {
			var t: number = -c / (2 * b);
			if (b > 0) {
				// (0, t)
				tminxz = 0;
				tmaxxz = t;
				if (t <= 0) return false;
			} else {
				// (t, 1)
				tminxz = t;
				tmaxxz = 1;
				if (t >= 1) return false;
			}
		}

		p1y += halfH; // revert translation

		var min: number;
		if (tmaxxz <= tminy || tmaxy <= tminxz) return false;
		if (tminxz < tminy) {
			min = tminy;
			if (min == 0) return false; // the ray starts from inside
			hit.normal.init(0, dy > 0 ? -1 : 1, 0);
		} else {
			min = tminxz;
			if (min == 0) return false; // the ray starts from inside
			hit.normal.init(p1x + dx * min, 0, p1z + dz * min).normalize().scaleEq(this.cosTheta);
			hit.normal.y += this.sinTheta;
		}

		hit.position.init(p1x + min * dx, p1y + min * dy, p1z + min * dz);
		hit.fraction = min;
		return true;
	}

}
